package com.example.dualcamera;

import android.annotation.SuppressLint;
import android.content.pm.ActivityInfo;
import android.content.res.Configuration;
import android.graphics.Bitmap;
import android.graphics.ImageFormat;
import android.graphics.Rect;
import android.graphics.YuvImage;
import android.os.Build;
import android.os.Bundle;
import android.hardware.Camera;
import android.os.Environment;
import android.support.annotation.RequiresApi;
import android.util.Log;
import android.view.KeyEvent;
import android.view.SurfaceHolder;
import android.view.SurfaceView;

import java.io.ByteArrayOutputStream;
import java.io.File;
import java.io.FileOutputStream;
import java.io.IOException;

import android.support.v7.app.AppCompatActivity;
import android.graphics.BitmapFactory;
import android.widget.Button;
import android.widget.TextView;

import com.vm.lib.NativeWarpper;

import org.opencv.android.BaseLoaderCallback;
import org.opencv.android.LoaderCallbackInterface;
import org.opencv.android.OpenCVLoader;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.CvType;
import org.opencv.core.Mat;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;

import java.nio.ByteBuffer;
import java.text.DecimalFormat;
import java.util.ArrayList;
import java.util.List;

import static java.lang.Math.round;


public class MainActivity extends AppCompatActivity implements Camera.PreviewCallback {
    private static final String TAG = "tofapp";

    static String strFormat = "ply";
    static String strAscii = "format ascii 1.0";
    static String strElements = "element vertex ";
    static String strProperX = "property float32 x";
    static String strProperY = "property float32 y";
    static String strProperZ = "property float32 z";
    static String strEnd = "end_header";

    SurfaceView surfaceview1, surfaceview2;
    SurfaceHolder surfaceholder1, surfaceholder2;
    private Camera camera1 = null, camera2;
    Camera.Parameters parameters;
    static int frameIdTOF = 0;
    static int frameIdRGB = 0;

    public int width = 224;
    public int height = 172;
    public double fx_tof = 217.869247;
    public double fy_tof = 217.869247;
    public double cx_tof = 110.485161;
    public double cy_tof = 84.037331;
    public double k1_tof = -0.136084;
    public double k2_tof = -1.520149;
    public double k3_tof = -0.000889;
    public double p1_tof = 0.000396;
    public double p2_tof = 3.170387;
    double f = (fx_tof + fy_tof) / 2.0;

    public TextView tv = null;
    public TextView tv2 = null;
    public TextView tv3 = null;

    static {
        System.loadLibrary("opencv_java3");
    }

    public class OpenCVNativeLoader {
        public void init() {
            System.loadLibrary("opencv_java3");

        }
    }

    private BaseLoaderCallback mLoaderCallback = new BaseLoaderCallback(this) {
        @Override
        public void onManagerConnected(int status) {
            switch (status) {
                case LoaderCallbackInterface.SUCCESS: {
                    Log.i(TAG, "OpenCV loaded successfully");

                }
                break;
                default: {
                    super.onManagerConnected(status);
                }
                break;
            }
        }
    };

    @Override
    public void onResume() {
        super.onResume();
        if (!OpenCVLoader.initDebug()) {
            Log.d(TAG, "Internal OpenCV library not found. Using OpenCV Manager for initialization");
            OpenCVLoader.initAsync(OpenCVLoader.OPENCV_VERSION_3_0_0, this, mLoaderCallback);
        } else {
            Log.d(TAG, "OpenCV library found inside package. Using it!");
            mLoaderCallback.onManagerConnected(LoaderCallbackInterface.SUCCESS);
        }
    }

    private OpenCVNativeLoader loader = new OpenCVNativeLoader();


    @Override
    protected void onCreate(Bundle savedInstanceState) {
        super.onCreate(savedInstanceState);
        setContentView(R.layout.activity_main);
        setRequestedOrientation(ActivityInfo.SCREEN_ORIENTATION_PORTRAIT);


        surfaceview1 = (SurfaceView) findViewById(R.id.surfaceview1);
        surfaceview2 = (SurfaceView) findViewById(R.id.surfaceview2);
        surfaceholder1 = surfaceview1.getHolder();
        surfaceholder1.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
        surfaceholder1.addCallback(new surfaceholderCallbackBack());

        surfaceholder2 = surfaceview2.getHolder();
        surfaceholder2.setType(SurfaceHolder.SURFACE_TYPE_PUSH_BUFFERS);
        surfaceholder2.addCallback(new surfaceholderCallbackFont());

        tv = (TextView) findViewById(R.id.textView);
        tv.setText("-1");
        tv2 = (TextView) findViewById(R.id.textView2);
        tv2.setText("?");

        tv3 = (TextView) findViewById(R.id.textView3);
        tv3.setText(mstr[mode]);


        NativeWarpper.GetPolygonMask(width, height, vertex, mask);
    }

    @Override
    protected void onDestroy() {
        Log.e(TAG, "onDestroy");
        super.onDestroy();
    }

    byte[] mRGBCameraData;
    long timeStampRGB;
    long timeStamp;
    boolean foundMatch = true;
    boolean skipData = false;


    public void writeSDFile(String fileName, byte[] bytes) throws IOException {

        String f = Environment.getExternalStorageDirectory().getPath();//获取SD卡目录
        String sDir = "thetaj";
        File destDir = new File(f + "/" + sDir);
        if (!destDir.exists()) {
            destDir.mkdirs();
        }

//        File file = new File(f+"/"+sDir+"/"+fileName);
//        FileOutputStream fos = new FileOutputStream(file);
//        fos.write(bytes);
////        fos.writeBytes()
//        fos.close();

        NativeWarpper.DecodeAndSave(bytes, f + "/" + sDir + "/" + fileName);
    }


    public float[] metric = new float[]{0, 0, 0};
    public float[] plane = new float[]{0, 0, 0, 0};
    public float[] K = new float[]{(float) fx_tof, 0, (float) cx_tof, 0, (float) fy_tof, (float) cy_tof, 0, 0, 1};
    public float[] D = new float[]{(float) k1_tof, (float) k2_tof, (float) k3_tof, (float) p1_tof, (float) p2_tof};
    public byte[] mask = new byte[width * height];

    public int[] vertex = new int[]{14,31,152,30,150,150,18,149};
    public int mode = 2;
    public String[] mstr=new String[]{"measure", "calibrate", "capture"};

    @Override
    public void onPreviewFrame(byte[] data, Camera camera) {

        if (camera.equals(camera2)) {

        } else {
            frameIdTOF++;
            if (mode == 0) {
                NativeWarpper.MeasureObject(metric, data, plane, K, D, mask, width, height, 10, 50);
                String msg = "[" + round(metric[0]) + "x" + round(metric[1]) + "x" + round(metric[2]) + "]mm";
                tv.setText(msg);
            }

            if (mode == 1) {
                NativeWarpper.CalibratePlane(plane, data, K, D, mask, width, height);
                String msg = plane[0] + ", " + plane[1] + ", " + plane[2] + ", " + plane[3] + ", ";
                tv2.setText(msg);
            }

            if (mode == 2) {

                try {
                    writeSDFile(frameIdTOF + "", data);
                } catch (IOException e) {
                    e.printStackTrace();
                }
            }

            tv3.setText(frameIdTOF + "      "+mstr[mode]);
        }
    }

    @Override
    public boolean onKeyDown(int keyCode, KeyEvent event) {

        switch (keyCode)
        {
            case KeyEvent.KEYCODE_VOLUME_DOWN:
                mode=(mode+2)%3;
                tv3.setText(frameIdTOF + "      "+mstr[mode]);
                return true;
            case KeyEvent.KEYCODE_VOLUME_UP:
                mode=(mode+1)%3;
                tv3.setText(frameIdTOF + "      "+mstr[mode]);
                return true;
        }


        return super.onKeyDown(keyCode, event);
    }
//
//    @Override
//    public void onPreviewFrame(final byte[] data, Camera camera) {
//
//        int dat4 = data[data.length - 5] & 0xFF;
//        int dat3 = data[data.length - 6] & 0xFF;
//        int dat2 = data[data.length - 7] & 0xFF;
//        int dat1 = data[data.length - 8] & 0xFF;
//        long timeusec = dat4 << 24 | dat3 << 16 | dat2 << 8 | dat1;
//        dat4 = data[data.length - 13] & 0xFF;
//        dat3 = data[data.length - 14] & 0xFF;
//        dat2 = data[data.length - 15] & 0xFF;
//        dat1 = data[data.length - 16] & 0xFF;
//        long timesec = dat4 << 24 | dat3 << 16 | dat2 << 8 | dat1;
//        timeStamp = timesec * 1000000 + timeusec;
//        if (camera.equals(camera2)) {
//            frameIdRGB++;
//
//
//            if (skipData || foundMatch) {
//                if (mRGBCameraData == null) {
//                    mRGBCameraData = new byte[data.length];
//                }
//                mRGBCameraData = data;
//                timeStampRGB = timeStamp;
//                foundMatch = false;
//                skipData = false;
//            }
//        } else {
//            frameIdTOF++;
//
//            if (foundMatch) {
//                return;
//            }
//            long during = timeStamp - timeStampRGB;
//            if (false && during < 0) {
//                foundMatch = false;
//            } else {
//                if (Math.abs(during) < 60 * 1000) {
//                    foundMatch = true;
////                    final int xxxx = frameIdRGB;
////                    new Thread(new Runnable() {
////                        @RequiresApi(api = Build.VERSION_CODES.O)
////                        @Override
////                        public void run() {
////                            int index = xxxx;
////
////
////
////                            savedepth(index, data);
////                            savemono(index, data);
////                            saveRGB(index, mRGBCameraData);
////
////
////                        }
////                    }).start();
//                } else {
//                    skipData = true;
//                }
//            }
//        }
//    }
//
//
//
//
//    public Mat savemono(int frameId, byte[] dataTOF) {
//        DecimalFormat f = new DecimalFormat("0000");
//        String str = f.format(frameId);
//        int offset = 224*172*2;
//        int size = 224*172;
//        byte[] mono = new byte[224*172];
//        System.arraycopy(dataTOF, offset, mono, 0, size);
//        Mat monodata = new Mat(172,224, CvType.CV_8UC1);
//        monodata.put(0,0,mono);
//        mono_rectify(monodata,str);
//        return monodata;
//
//    }
//
//    public Mat mono_rectify(Mat mono, String str) {
//        int width = mono.width();
//        int height = mono.height();
//        Size size = new Size(width, height);
//        float[] matx =  new float[] {(float) fx_tof,0, (float) cx_tof,0, (float) fy_tof, (float) cy_tof,0,0,1};
//        Mat mtx = new Mat (3,3,CvType.CV_32F);
//        mtx.put(0,0,matx);
//        Mat dist= new Mat(1,5,CvType.CV_32F);
//        dist.put(0,0,new float[]{(float)k1_tof,(float)k2_tof,(float)k3_tof,(float)p1_tof,(float)p2_tof});
//        Mat newcameramtx = Calib3d.getOptimalNewCameraMatrix(mtx, dist,size,0);
//        Mat mono_rectify = new Mat();
//        Mat map1 = new Mat();
//        Mat map2 = new Mat();
//        float[] a =  new float[] {1,0,0,0,1,0,0,0,1};
//        Mat R = new Mat (3,3,CvType.CV_32F);
//        R.put(0,0,a);
//        Imgproc.initUndistortRectifyMap(mtx,dist,R,newcameramtx,size,CvType.CV_32FC1,map1,map2);
//        Imgproc.remap(mono, mono_rectify, map1, map2, Imgproc.INTER_LINEAR);
//        return mono_rectify;
//    }
//
//    public Bitmap saveRGB(int frameId, byte[] dataRGB) {
//        YuvImage yuvimage = new YuvImage(dataRGB, ImageFormat.NV21, 2064, 1548, null);
//        ByteArrayOutputStream baos = new ByteArrayOutputStream();
//        yuvimage.compressToJpeg(new Rect(0, 0,2064, 1548), 80, baos);
//        Bitmap rgbimage = BitmapFactory.decodeByteArray(baos.toByteArray(), 0, baos.toByteArray().length);
//        return rgbimage;
//    }
//
//    @RequiresApi(api = Build.VERSION_CODES.O)
//    public Mat savedepth(int frameId, byte[] dataTOF) {
//        DecimalFormat f = new DecimalFormat("0000");
//        String str = f.format(frameId);
//        int offset = 0;
//        int size = 224*172*2;
//        byte[] depth = new byte[size];
//        System.arraycopy(dataTOF, offset, depth, 0, size);
//        ByteBuffer byteBuffer = ByteBuffer.allocateDirect(depth.length );
//        byteBuffer.put(depth);
//        Mat depthimg = new Mat(172,224, CvType.CV_16U,byteBuffer);
//        depth_rectify(depthimg,str);
//        return depthimg;
//
//    }
//    @RequiresApi(api = Build.VERSION_CODES.O)
//    public Mat depth_rectify(Mat depth, String str) {
//        int width = depth.width();
//        int height = depth.height();
//        Size size = new Size(width, height);
//        float[] matx =  new float[] {(float) fx_tof,0, (float) cx_tof,0, (float) fy_tof, (float) cy_tof,0,0,1};
//        Mat mtx = new Mat (3,3,CvType.CV_32F);
//        mtx.put(0,0,matx);
//        Mat dist= new Mat(1,5,CvType.CV_32F);
//        dist.put(0,0,new float[]{(float)-0.136084,(float)-1.520149,(float)-0.000889,(float)0.000396,(float)3.170387});
//        Mat newcameramtx = Calib3d.getOptimalNewCameraMatrix(mtx, dist,size,0);
//        Mat depth_rectify = new Mat();
//        Mat map1 = new Mat();
//        Mat map2 = new Mat();
//        float[] a =  new float[] {1,0,0,0,1,0,0,0,1};
//        Mat R = new Mat (3,3,CvType.CV_32F);
//        R.put(0,0,a);
//        Imgproc.initUndistortRectifyMap(mtx,dist,R,newcameramtx,size,CvType.CV_32FC1,map1,map2);
//        Imgproc.remap(depth, depth_rectify, map1, map2, Imgproc.INTER_LINEAR);
//        depth2ply(depth_rectify,str);
//        return depth_rectify;
//    }
//
//    @RequiresApi(api = Build.VERSION_CODES.O)
//    public void depth2ply(Mat depthimg, String str) {
//        List<String> list = new ArrayList<String>();
//        short[] depth = new short[depthimg.cols() *depthimg.rows() ];
//        int res = depthimg.get(0,0,depth);
//
//        for(int i = 0; i<depthimg.cols(); i++) {
//            for (int j = 0; j < depthimg.rows(); j++) {
//                float disp = (float) ((depth[depthimg.cols() * j + i]) & 0x1fff);
//                if (disp != 0) {
//
//                    double x = ( j - cx_tof ) / fx_tof * disp;
//                    double y = ( i - cy_tof ) / fy_tof * disp;
//                    String str1 = "" + x;
//                    String str2 = "" + y;
//                    String str3 = "" + disp;
//
////                        double z = ( 90 * f )/( 0.15 * f + ( disp - 1024 ) * 0.1125 );
////                        double x = ( j- cx_tof/2  )* z * 2.0 / f;
////                        double y = ( i- cy_tof/2  )* z * 2.0 / f;
////                        String str1 = "" + x;
////                        String str2 = "" + y;
////                        String str3 = "" + z;
//                    list.add(str1+" "+str2+" "+str3+" \n");
//                    }
//                }
//            }
//        byte buffer1[]=new String("ply\n").getBytes();
//        byte buffer2[]=new String("format ascii 1.0\n").getBytes();
//        byte buffer3[]=new String("element vertex "+list.size()+"\n").getBytes();
//        byte buffer4[]=new String("property float x\n").getBytes();
//        byte buffer5[]=new String("property float y\n").getBytes();
//        byte buffer6[]=new String("property float z\n").getBytes();
//        byte buffer7[]=new String("end_header\n").getBytes();
//        String string = String.join(" ",list);
//        list.clear();
//
//    }


    /**
     * 后置摄像头回调
     */
    class surfaceholderCallbackBack implements SurfaceHolder.Callback {
        @Override
        public void surfaceCreated(SurfaceHolder holder) {
            // 获取camera对象
            int cameraCount = Camera.getNumberOfCameras();
            if (cameraCount > 0) {
                camera1 = Camera.open(0);
                try {
                    // 设置预览监听
                    camera1.setPreviewDisplay(holder);
                    Camera.Parameters parameters = camera1.getParameters();

                    if (MainActivity.this.getResources().getConfiguration().orientation != Configuration.ORIENTATION_LANDSCAPE) {
                        parameters.set("orientation", "portrait");
                        camera1.setDisplayOrientation(90);
                        parameters.setRotation(90);
                    } else {
                        parameters.set("orientation", "landscape");
                        camera1.setDisplayOrientation(0);
                        parameters.setRotation(0);
                    }
                    camera1.setParameters(parameters);
                    // 启动摄像头预览
                    camera1.setPreviewCallback(MainActivity.this);
                    camera1.startPreview();
                    System.out.println("camera.startpreview");

                } catch (IOException e) {
                    e.printStackTrace();
                    camera1.release();
                    System.out.println("camera.release");
                }
            }
        }

        @Override
        public void surfaceChanged(SurfaceHolder holder, int format, int width, int height) {

        }

        @Override
        public void surfaceDestroyed(SurfaceHolder holder) {
            camera1.setPreviewCallback(null);
            camera1.stopPreview();
            camera1.release();
        }
    }

    class surfaceholderCallbackFont implements SurfaceHolder.Callback {
        @Override
        public void surfaceCreated(SurfaceHolder holder) {
            // 获取camera对象
            int cameraCount = Camera.getNumberOfCameras();
            if (cameraCount == 2) {
                camera2 = Camera.open(1);
            }
            try {
                // 设置预览监听
                camera2.setPreviewDisplay(holder);
                Camera.Parameters parameters = camera2.getParameters();

                if (MainActivity.this.getResources().getConfiguration().orientation != Configuration.ORIENTATION_LANDSCAPE) {
                    parameters.set("orientation", "portrait");
                    camera2.setDisplayOrientation(90);
                    parameters.setRotation(90);
                } else {
                    parameters.set("orientation", "landscape");
                    camera2.setDisplayOrientation(0);
                    parameters.setRotation(0);
                }
                camera2.setParameters(parameters);
                camera2.setPreviewCallback(MainActivity.this);
                camera2.startPreview();
                System.out.println("camera.startpreview");

            } catch (IOException e) {
                e.printStackTrace();
                camera2.release();
                System.out.println("camera.release");
            }
        }

        @Override
        public void surfaceChanged(SurfaceHolder holder, int format, int width, int height) {

        }

        @Override
        public void surfaceDestroyed(SurfaceHolder holder) {
            camera2.setPreviewCallback(null);
            camera2.stopPreview();
        }
    }
}